#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#define RANDZEROONE drand48()
#define RANDONEONE ((2.0f*drand48())-1.0f)
#define RANDZEROFIVE (drand48()-0.5f)

extern REAL two;
extern REAL h;
extern REAL zerotwentyfive;
extern REAL Beta;
extern REAL one;


void loopXBlueHB(REAL * __restrict__ a, REAL * __restrict__ b, REAL * __restrict__ c,
               REAL * __restrict__ an, REAL * __restrict__ bn, REAL * __restrict__ cn,
               REAL * __restrict__ Jpx, REAL * __restrict__ Jpy, REAL * __restrict__ Jpz,
               REAL * __restrict__ Jmx, REAL * __restrict__ Jmy, REAL * __restrict__ Jmz,
               REAL * __restrict__ t_a, REAL * __restrict__ t_b, REAL * __restrict__ t_c,
               int istart, int iend, int yzoff,
               int spxoff, int smxoff, int spyoff, int smyoff, int spzoff, int smzoff) {
	
	int ix;
	//int spx, smx, spy, smy, spz, smz;
	REAL as, bs, cs;
	REAL factor;
	
	REAL anew, bnew, cnew;
	REAL hk, bhk, projection, pl, invnorm;
	
	REAL invexpbhk2;
	
	REAL * __restrict__ anspx=an+spxoff, * __restrict__ ansmx=an+smxoff;
	REAL * __restrict__ anspy=an+spyoff, * __restrict__ ansmy=an+smyoff;
	REAL * __restrict__ anspz=an+spzoff, * __restrict__ ansmz=an+smzoff;
	
	REAL * __restrict__ bnspx=bn+spxoff, * __restrict__ bnsmx=bn+smxoff;
	REAL * __restrict__ bnspy=bn+spyoff, * __restrict__ bnsmy=bn+smyoff;
	REAL * __restrict__ bnspz=bn+spzoff, * __restrict__ bnsmz=bn+smzoff;
	
	REAL * __restrict__ cnspx=cn+spxoff, * __restrict__ cnsmx=cn+smxoff;
	REAL * __restrict__ cnspy=cn+spyoff, * __restrict__ cnsmy=cn+smyoff;
	REAL * __restrict__ cnspz=cn+spzoff, * __restrict__ cnsmz=cn+smzoff;
	
	REAL * __restrict__ JpxWO=Jpx+spxoff, * __restrict__ JmxWO=Jmx+smxoff;
	REAL * __restrict__ JpyWO=Jpy+spyoff, * __restrict__ JmyWO=Jmy+smyoff;
	REAL * __restrict__ JpzWO=Jpz+spzoff, * __restrict__ JmzWO=Jmz+smzoff;
	
	REAL * __restrict__ at=a+yzoff, * __restrict__ bt=b+yzoff, * __restrict__ ct=c+yzoff;
	REAL * __restrict__ ta=t_a+yzoff, * __restrict__ tb=t_b+yzoff,
	* __restrict__ tc=t_c+yzoff;
	
	
	for(ix=istart; ix<iend; ix++) {
		
	//	printf("%d\n",ix);
		
		anew = RANDZEROFIVE;
		bnew = RANDZEROFIVE;
		
		pl=anew*anew+bnew*bnew;
		
		if(pl>zerotwentyfive) {
			continue;
		}
		
		cnew = RANDONEONE;
		
		as=anspx[ix]*JpxWO[ix]+
		anspy[ix]*JpyWO[ix]+
		anspz[ix]*JpzWO[ix]+
		ansmx[ix]*JmxWO[ix]+
		ansmy[ix]*JmyWO[ix]+
		ansmz[ix]*JmzWO[ix];
		
		bs=bnspx[ix]*JpxWO[ix]+
		bnspy[ix]*JpyWO[ix]+
		bnspz[ix]*JpzWO[ix]+
		bnsmx[ix]*JmxWO[ix]+
		bnsmy[ix]*JmyWO[ix]+
		bnsmz[ix]*JmzWO[ix];
		
		cs=cnspx[ix]*JpxWO[ix]+
		cnspy[ix]*JpyWO[ix]+
		cnspz[ix]*JpzWO[ix]+
		cnsmx[ix]*JmxWO[ix]+
		cnsmy[ix]*JmyWO[ix]+
		cnsmz[ix]*JmzWO[ix]+h;
		
		hk=SQRT(as*as+bs*bs+cs*cs);
		bhk=Beta*hk;
		
		invexpbhk2=EXP(-two*bhk);
		projection=one+(LOG(invexpbhk2+(one-invexpbhk2)*RANDZEROONE))/bhk;
		if(projection<-one) { projection=-one; }
		
		invnorm=SQRT((one-cnew*cnew)/pl);
		
		anew*=invnorm;
		bnew*=invnorm;
		
		factor=(as*anew+bs*bnew+cs*cnew)/(hk*hk);
		
		anew = anew-factor*as;
		bnew = bnew-factor*bs;
		cnew = cnew-factor*cs;
		
		factor=SQRT((one-projection*projection)/(anew*anew+bnew*bnew+cnew*cnew));
		
		projection/=hk;
		
		
	//	printf("%f %f %f %f %f %f %f %f\n",as,bs,cs,anew,bnew,cnew,projection,factor);

		
		ta[ix]=as*projection+anew*factor;
		tb[ix]=bs*projection+bnew*factor;
		tc[ix]=cs*projection+cnew*factor;
		
		/*ta[ix]=as*factor-at[ix];
		 tb[ix]=bs*factor-bt[ix];
		 tc[ix]=cs*factor-ct[ix];*/
		
		
	}
}
